#include "sensors_layer.h"
// #include <sensor_msgs/point_cloud_conversion.h>
#include <sensor_msgs/point_cloud2_iterator.h>

namespace costmap_2d{

SensorsLayer::SensorsLayer(){
    costmap_ = NULL;
}

SensorsLayer::~SensorsLayer(){

}


void SensorsLayer::onInitialize(){
    rolling_window_ = layered_costmap_->isRolling();
    global_frame_ = layered_costmap_->getGlobalFrameID();
}

void SensorsLayer::matchSize(){
    LOG(INFO) << "matchSize...... start" << endl;
    Costmap2D* master = layered_costmap_->getCostmap2D();
    resizeMap(master->getWidth(), master->getHeight(), master->getResolution(),
            master->getOriginX(), master->getOriginY());
    LOG(INFO) << "matchSize...... over" << endl;
}

void SensorsLayer::updateBounds(double robot_x, double robot_y, double robot_yaw, double* min_x, double* min_y,
    double* max_x, double* max_y){
    // LOG(INFO) << "updateBounds......" << endl;
    if (rolling_window_)
        updateOrigin(robot_x - getWidthInMeters() / 2, robot_y - getHeightInMeters() / 2);
    
    std::vector<Observation> observations;
    for (auto it = observation_buffers_.begin(); it != observation_buffers_.end(); ++it){
        it->second->lock();
        it->second->getObservations(observations);
        it->second->unlock();
    }

    for (unsigned int i = 0; i < observations.size(); ++i){
        raytraceFreespace(observations[i], min_x, min_y, max_x, max_y);
    }

    for (std::vector<Observation>::const_iterator it = observations.begin(); it != observations.end(); ++it){
        const Observation& obs = *it;
        const sensor_msgs::PointCloud2& cloud = *(obs.cloud_);
        double sq_obstacle_range = obs.obstacle_range_ * obs.obstacle_range_;
    
        sensor_msgs::PointCloud2ConstIterator<float> iter_x(cloud, "x");
        sensor_msgs::PointCloud2ConstIterator<float> iter_y(cloud, "y");
        sensor_msgs::PointCloud2ConstIterator<float> iter_z(cloud, "z");
    
        for (; iter_x !=iter_x.end(); ++iter_x, ++iter_y, ++iter_z){
            double px = *iter_x, py = *iter_y, pz = *iter_z;
        
            // if the obstacle is too high or too far away from the robot we won't add it
            // if (pz > max_obstacle_height_)
            // {
            //     ROS_DEBUG("The point is too high");
            //     continue;
            // }
        
            // compute the squared distance from the hitpoint to the pointcloud's origin
            double sq_dist = (px - obs.origin_.x) * (px - obs.origin_.x) + (py - obs.origin_.y) * (py - obs.origin_.y)
                + (pz - obs.origin_.z) * (pz - obs.origin_.z);
        
            // if the point is far enough away... we won't consider it
            if (sq_dist >= sq_obstacle_range)
            {
                ROS_DEBUG("The point is too far away");
                continue;
            }
        
            // now we need to compute the map coordinates for the observation
            unsigned int mx, my;
            if (!worldToMap(px, py, mx, my))
            {
                ROS_DEBUG("Computing map coords failed");
                continue;
            }
        
            unsigned int index = getIndex(mx, my);
            costmap_[index] = costmap_2d::LETHAL_OBSTACLE;
            layered_costmap_->touch(px, py, min_x, min_y, max_x, max_y);
        }      
    }
    // LOG(INFO) << "updateBounds...... over" << endl;
}

void SensorsLayer::updateCosts(costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j){
    // layered_costmap_->updateWithMax(master_grid, min_i, min_j, max_i, max_j);
    // LOG(INFO) << "updateCosts...... start" << endl;
    unsigned char* master_array = master_grid.getCharMap();
    unsigned int span = master_grid.getWidth();

    // LOG(INFO) << "master_array size: " << (master_array == NULL)
    //     << ", costmap_ size: " << (costmap_ == NULL);
  
    for (int j = min_j; j < max_j; j++)
    {
      unsigned int it = j * span + min_i;
      for (int i = min_i; i < max_i; i++)
      {
        if (costmap_[it] == costmap_2d::NO_INFORMATION){
          it++;
          continue;
        }
  
        unsigned char old_cost = master_array[it];
        if (old_cost == costmap_2d::NO_INFORMATION || old_cost < costmap_[it])
          master_array[it] = costmap_[it];
        it++;
      }
    }
    // LOG(INFO) << "updateCosts...... over" << endl;
}

void SensorsLayer::raytraceFreespace(const Observation& clearing_observation, double* min_x, double* min_y,
    double* max_x, double* max_y){
    double ox = clearing_observation.origin_.x;
    double oy = clearing_observation.origin_.y;
    const sensor_msgs::PointCloud2 &cloud = *(clearing_observation.cloud_);

    // get the map coordinates of the origin of the sensor
    unsigned int x0, y0;
    if (!worldToMap(ox, oy, x0, y0)){
        ROS_WARN_THROTTLE(
        1.0, "The origin for the sensor at (%.2f, %.2f) is out of map bounds. So, the costmap cannot raytrace for it.",
        ox, oy);
        return;
    }

    // we can pre-compute the enpoints of the map outside of the inner loop... we'll need these later
    double origin_x = origin_x_, origin_y = origin_y_;
    double map_end_x = origin_x + width_ * resolution_;
    double map_end_y = origin_y + height_ * resolution_;

    layered_costmap_->touch(ox, oy, min_x, min_y, max_x, max_y);

    // for each point in the cloud, we want to trace a line from the origin and clear obstacles along it
    sensor_msgs::PointCloud2ConstIterator<float> iter_x(cloud, "x");
    sensor_msgs::PointCloud2ConstIterator<float> iter_y(cloud, "y");

    for (; iter_x != iter_x.end(); ++iter_x, ++iter_y){
        double wx = *iter_x;
        double wy = *iter_y;

        // now we also need to make sure that the enpoint we're raytracing
        // to isn't off the costmap and scale if necessary
        double a = wx - ox;
        double b = wy - oy;

        // the minimum value to raytrace from is the origin
        if (wx < origin_x)
        {
        double t = (origin_x - ox) / a;
        wx = origin_x;
        wy = oy + b * t;
        }
        if (wy < origin_y)
        {
        double t = (origin_y - oy) / b;
        wx = ox + a * t;
        wy = origin_y;
        }

        // the maximum value to raytrace to is the end of the map
        if (wx > map_end_x)
        {
        double t = (map_end_x - ox) / a;
        wx = map_end_x - .001;
        wy = oy + b * t;
        }
        if (wy > map_end_y)
        {
        double t = (map_end_y - oy) / b;
        wx = ox + a * t;
        wy = map_end_y - .001;
        }

        // now that the vector is scaled correctly... we'll get the map coordinates of its endpoint
        unsigned int x1, y1;

        // check for legality just in case
        if (!worldToMap(wx, wy, x1, y1))
        continue;

        unsigned int cell_raytrace_range = cellDistance(clearing_observation.raytrace_range_);
        MarkCell marker(costmap_, FREE_SPACE);
        // and finally... we can execute our trace to clear obstacles along that line
        raytraceLine(marker, x0, y0, x1, y1, cell_raytrace_range);

        updateRaytraceBounds(ox, oy, wx, wy, clearing_observation.raytrace_range_, min_x, min_y, max_x, max_y);
    }
}

void SensorsLayer::updateRaytraceBounds(double ox, double oy, double wx, double wy, double range,
    double* min_x, double* min_y, double* max_x, double* max_y){
    double dx = wx-ox, dy = wy-oy;
    double full_distance = hypot(dx, dy);
    double scale = std::min(1.0, range / full_distance);
    double ex = ox + dx * scale, ey = oy + dy * scale;
    layered_costmap_->touch(ex, ey, min_x, min_y, max_x, max_y);
}

void SensorsLayer::createBuffer(boost::shared_ptr<costmap_2d::ObservationBuffer> buffer, string topic_name){
    observation_buffers_[topic_name] = buffer;
}

void SensorsLayer::addPointCloud2(sensor_msgs::PointCloud2 cloud, string topic_name){
    // LOG(INFO) << "[addPointCloud2]: " << topic_name;
    observation_buffers_[topic_name]->lock();
    observation_buffers_[topic_name]->bufferCloud(cloud);
    observation_buffers_[topic_name]->unlock();
}

} // namespace costmap_2d